package com.zygh.lz.stapprove;


import com.zygh.lz.entity.vo.CoordinateData;

import java.util.ArrayList;
import java.util.List;

/**
 * 轨迹优化工具类
 * Created by my94493 on 2017/3/31.
 * <p>
 * 使用方法：
 * <p>
 * PathSmoothTool pathSmoothTool = new PathSmoothTool();
 * pathSmoothTool.setIntensity(2);//设置滤波强度，默认3
 * List<CoordinateData> mList = LatpathSmoothTool.kalmanFilterPath(list);
 * 3  0.3  10
 */
public class PathSmoothTool {
    private int mIntensity = 10;
    private float mThreshhold = 1.3f;
    //private int mThreshhold = 25;
    private float mNoiseThreshhold = 10;

    public PathSmoothTool() {

    }


    /**
     * 轨迹平滑优化
     *
     * @param originlist 原始轨迹list,list.size大于2
     * @return 优化后轨迹list
     */
    public List<CoordinateData> pathOptimize(List<CoordinateData> originlist) {

        List<CoordinateData> list = removeNoisePoint(originlist);//去噪
        //System.out.println("------------去燥"+list);
        List<CoordinateData> afterList = kalmanFilterPath(list, mIntensity);//滤波
        System.out.println("@@@@@@@@@@滤波" + afterList);
        //List<CoordinateData> pathoptimizeList = DouglasPeuckerUtil.DouglasPeucker(afterList, mThreshhold);//抽稀
        List<CoordinateData> pathoptimizeList = reducerVerticalThreshold(afterList, mThreshhold);//抽稀
        return pathoptimizeList;
    }

    /**
     * 轨迹线路滤波
     *
     * @param originlist 原始轨迹list,list.size大于2
     * @return 滤波处理后的轨迹list
     */
    public List<CoordinateData> kalmanFilterPath(List<CoordinateData> originlist) {
        return kalmanFilterPath(originlist, mIntensity);
    }


    /**
     * 轨迹去噪，删除垂距大于20m的点
     *
     * @param originlist 原始轨迹list,list.size大于2
     * @return
     */
    public List<CoordinateData> removeNoisePoint(List<CoordinateData> originlist) {
        return reduceNoisePoint(originlist, mNoiseThreshhold);
    }

    /**
     * 单点滤波
     *
     * @param lastLoc 上次定位点坐标
     * @param curLoc  本次定位点坐标
     * @return 滤波后本次定位点坐标值
     */
//    public CoordinateData kalmanFilterPoint(CoordinateData lastLoc, CoordinateData curLoc, int intensity) {
//        return kalmanFilterPoint(lastLoc, curLoc, mIntensity);
//    }


    /**
     * 轨迹抽稀
     *
     * @param inPoints 待抽稀的轨迹list，至少包含两个点，删除垂距小于mThreshhold的点
     * @return 抽稀后的轨迹list
     */
    public List<CoordinateData> reducerVerticalThreshold(List<CoordinateData> inPoints) {
        return reducerVerticalThreshold(inPoints, mThreshhold);
    }

    /********************************************************************************************************/
    /**
     * 轨迹线路滤波
     *
     * @param originlist 原始轨迹list,list.size大于2
     * @param intensity  滤波强度（1—5）
     * @return
     */
    private List<CoordinateData> kalmanFilterPath(List<CoordinateData> originlist, int intensity) {
        List<CoordinateData> kalmanFilterList = new ArrayList<CoordinateData>();
        if (originlist == null || originlist.size() <= 2)
            return kalmanFilterList;
        initial();//初始化滤波参数
        CoordinateData latLng = null;
        CoordinateData lastLoc = originlist.get(0);
        kalmanFilterList.add(lastLoc);
        for (int i = 1; i < originlist.size(); i++) {
            CoordinateData curLoc = originlist.get(i);
            latLng = kalmanFilterPoint(lastLoc, curLoc, intensity);
            if (latLng != null) {
                kalmanFilterList.add(latLng);
                lastLoc = latLng;
            }
        }
        return kalmanFilterList;
    }

    /**
     * 单点滤波
     *
     * @param lastLoc   上次定位点坐标
     * @param curLoc    本次定位点坐标
     * @param intensity 滤波强度（1—5）
     * @return 滤波后本次定位点坐标值
     */
    public CoordinateData kalmanFilterPoint(CoordinateData lastLoc, CoordinateData curLoc, int intensity) {
        if (pdelt_x == 0 || pdelt_y == 0) {
            initial();
        }
        CoordinateData kalmanLatlng = null;
        if (lastLoc == null || curLoc == null) {
            return kalmanLatlng;
        }
        if (intensity < 1) {
            intensity = 1;
        } else if (intensity > 5) {
            intensity = 5;
        }
        for (int j = 0; j < intensity; j++) {
            kalmanLatlng = kalmanFilter(lastLoc.getLongitude(), curLoc.getLongitude(), lastLoc.getLatitude(), curLoc.getLatitude());
            curLoc = kalmanLatlng;
        }
        return kalmanLatlng;
    }


    /***************************卡尔曼滤波开始********************************/
    private double lastLocation_x; //上次位置
    private double currentLocation_x;//这次位置
    private double lastLocation_y; //上次位置
    private double currentLocation_y;//这次位置
    private double estimate_x; //修正后数据
    private double estimate_y; //修正后数据
    private double pdelt_x; //自预估偏差
    private double pdelt_y; //自预估偏差
    private double mdelt_x; //上次模型偏差
    private double mdelt_y; //上次模型偏差
    private double gauss_x; //高斯噪音偏差
    private double gauss_y; //高斯噪音偏差
    private double kalmanGain_x; //卡尔曼增益
    private double kalmanGain_y; //卡尔曼增益

    private double m_R = 0;
    private double m_Q = 0;

    //初始模型
    private void initial() {
        pdelt_x = 0.001;
        pdelt_y = 0.001;
//        mdelt_x = 0;
//        mdelt_y = 0;
        mdelt_x = 5.698402909980532E-4;
        mdelt_y = 5.698402909980532E-4;
    }

    private CoordinateData kalmanFilter(double oldValue_x, double value_x, double oldValue_y, double value_y) {
        lastLocation_x = oldValue_x;
        currentLocation_x = value_x;
        gauss_x = Math.sqrt(pdelt_x * pdelt_x + mdelt_x * mdelt_x) + m_Q;     //计算高斯噪音偏差
        kalmanGain_x = Math.sqrt((gauss_x * gauss_x) / (gauss_x * gauss_x + pdelt_x * pdelt_x)) + m_R; //计算卡尔曼增益
        estimate_x = kalmanGain_x * (currentLocation_x - lastLocation_x) + lastLocation_x;    //修正定位点
        mdelt_x = Math.sqrt((1 - kalmanGain_x) * gauss_x * gauss_x);      //修正模型偏差

        lastLocation_y = oldValue_y;
        currentLocation_y = value_y;
        gauss_y = Math.sqrt(pdelt_y * pdelt_y + mdelt_y * mdelt_y) + m_Q;     //计算高斯噪音偏差
        kalmanGain_y = Math.sqrt((gauss_y * gauss_y) / (gauss_y * gauss_y + pdelt_y * pdelt_y)) + m_R; //计算卡尔曼增益
        estimate_y = kalmanGain_y * (currentLocation_y - lastLocation_y) + lastLocation_y;    //修正定位点
        mdelt_y = Math.sqrt((1 - kalmanGain_y) * gauss_y * gauss_y);      //修正模型偏差

        //CoordinateData latlng = new CoordinateData(estimate_y, estimate_x);
        CoordinateData latlng = new CoordinateData();
        latlng.setLongitude(estimate_x);
        latlng.setLatitude(estimate_y);

        return latlng;
    }
    /***************************卡尔曼滤波结束**********************************/

    /***************************抽稀算法*************************************/
    private List<CoordinateData> reducerVerticalThreshold(List<CoordinateData> inPoints,
                                                          float threshHold) {
        if (inPoints == null) {
            return null;
        }
        if (inPoints.size() <= 2) {
            return inPoints;
        }
        List<CoordinateData> ret = new ArrayList<CoordinateData>();
        for (int i = 0; i < inPoints.size(); i++) {
            CoordinateData pre = getLastLocation(ret);
            CoordinateData cur = inPoints.get(i);
            if (pre == null || i == inPoints.size() - 1) {
                ret.add(cur);
                continue;
            }
            CoordinateData next = inPoints.get(i + 1);
            double distance = calculateDistanceFromPoint(cur, pre, next);
            if (distance > threshHold) {
                ret.add(cur);
            }
        }
        return ret;
    }


    private static CoordinateData getLastLocation(List<CoordinateData> oneGraspList) {
        if (oneGraspList == null || oneGraspList.size() == 0) {
            return null;
        }
        int locListSize = oneGraspList.size();
        CoordinateData lastLocation = oneGraspList.get(locListSize - 1);
        return lastLocation;
    }

    /**
     * 计算当前点到线的垂线距离
     *
     * @param p         当前点
     * @param lineBegin 线的起点
     * @param lineEnd   线的终点
     */
    private static double calculateDistanceFromPoint(CoordinateData p, CoordinateData lineBegin,
                                                     CoordinateData lineEnd) {
        double A = p.getLongitude() - lineBegin.getLongitude();
        double B = p.getLatitude() - lineBegin.getLatitude();
        double C = lineEnd.getLongitude() - lineBegin.getLongitude();
        double D = lineEnd.getLatitude() - lineBegin.getLatitude();

        double dot = A * C + B * D;
        double len_sq = C * C + D * D;
        double param = dot / len_sq;

        double xx, yy;

        if (param < 0 || (lineBegin.getLongitude() == lineEnd.getLongitude()
                && lineBegin.getLatitude() == lineEnd.getLatitude())) {
            xx = lineBegin.getLongitude();
            yy = lineBegin.getLatitude();
//            return -1;
        } else if (param > 1) {
            xx = lineEnd.getLongitude();
            yy = lineEnd.getLatitude();
//            return -1;
        } else {
            xx = lineBegin.getLongitude() + param * C;
            yy = lineBegin.getLatitude() + param * D;

        }
        CoordinateData coordinateData = new CoordinateData();
        coordinateData.setLatitude(yy);
        coordinateData.setLongitude(xx);
        return calculateLineDistance(p, coordinateData);
    }

    /***************************抽稀算法结束*********************************/

    private List<CoordinateData> reduceNoisePoint(List<CoordinateData> inPoints, float threshHold) {
        if (inPoints == null) {
            return null;
        }
        if (inPoints.size() <= 2) {
            return inPoints;
        }
        List<CoordinateData> ret = new ArrayList<CoordinateData>();
        for (int i = 0; i < inPoints.size(); i++) {
            CoordinateData pre = getLastLocation(ret);
            CoordinateData cur = inPoints.get(i);
            if (pre == null || i == inPoints.size() - 1) {
                ret.add(cur);
                continue;
            }
            CoordinateData next = inPoints.get(i + 1);
            double distance = calculateDistanceFromPoint(cur, pre, next);
            if (distance < threshHold) {
                ret.add(cur);
            }
        }
        return ret;
    }

    public static float calculateLineDistance(CoordinateData var0, CoordinateData var1) {
        if (var0 != null && var1 != null) {
            try {
                double var2 = var0.getLongitude();
                double var4 = var0.getLatitude();
                double var6 = var1.getLongitude();
                double var8 = var1.getLatitude();
                var2 *= 0.01745329251994329D;
                var4 *= 0.01745329251994329D;
                var6 *= 0.01745329251994329D;
                var8 *= 0.01745329251994329D;
                double var10 = Math.sin(var2);
                double var12 = Math.sin(var4);
                double var14 = Math.cos(var2);
                double var16 = Math.cos(var4);
                double var18 = Math.sin(var6);
                double var20 = Math.sin(var8);
                double var22 = Math.cos(var6);
                double var24 = Math.cos(var8);
                double[] var26 = new double[3];
                double[] var27 = new double[3];
                var26[0] = var16 * var14;
                var26[1] = var16 * var10;
                var26[2] = var12;
                var27[0] = var24 * var22;
                var27[1] = var24 * var18;
                var27[2] = var20;
                double var28 = Math.sqrt((var26[0] - var27[0]) * (var26[0] - var27[0]) + (var26[1] - var27[1]) * (var26[1] - var27[1]) + (var26[2] - var27[2]) * (var26[2] - var27[2]));
                return (float) (Math.asin(var28 / 2.0D) * 1.27420015798544E7D);
            } catch (Throwable var30) {
                var30.printStackTrace();
                return 0.0F;
            }
        } else {
            try {
                throw new Exception("非法坐标值");
            } catch (Exception var31) {
                var31.printStackTrace();
                return 0.0F;
            }
        }
    }
}

